#include "includes.h"

extern FSM gramb;
extern FSM_EVENT gramb_event;

/* amp[0]: left(C2) ADC0_SE11
 * amp[1]: right(B1) ADC0_SE9
 * amp[2]: left_head(E20) ADC0_DADP0
 * amp[3]: right_head(E22) ADC0_DADP3
 */
uint16 amp[4];

/* cnt[0]: left(B16) TPM_CLKIN0
 * cnt[1]: right(C5) LPTMR_ALT2
 */
int cnt[2];
extern int motor_t;

double uni[4];
const uint8_t amp_ad[AMP_ADC_CHN+1]={ADC_AD9, ADC_AD11, ADC_DADP3, ADC_DADP0, 0x1F};

#if FILTER==1
/* simple moving average */
int amp_int[4];
int amp_seq[4][AMP_T];

void moving_average(void){
    int i,n;
    static int t;

    for(n=2; n>=0; --n){
        for(i=0; i<4; ++i){
            amp_int[i]-=amp_seq[i][t];
            amp_seq[i][t]=amp[i];
            amp_int[i]+=amp_seq[i][t];
        }
        t=IN_RANGE(t+1, AMP_T);
    }

    for(i=0; i<4; ++i)
        amp[i]=amp_int[i]/AMP_T;
}
#elif FILTER==3
/* kalman filter */
void kalman(void){
    int i;
    static double x_[4],x[4];
    static double p_[4],p[4];
    static double k[4];
    static double q=0.1;
    static double r=5;
    for(i=0; i<4; ++i){
            x_[i]=x[i];
            p_[i]=p[i]+q;
            k[i]=p_[i]/(p_[i]+r);
            x[i]=x_[i]+k[i]*(amp[i]-x_[i]);
            p[i]=(1-k[i])*p_[i];
        }
    for(i=0; i<4; ++i)
        amp[i]=(uint16)(x[i]+0.5);
}
#endif

void speed_detection(void){
    cnt[0]=tpm_cnt_read(TPM2);
    cnt[1]=lptmr_read();
}

void data_unify(void){
    int i;

    for(i=0; i<4; ++i){
        uni[i]=amp[i];
        uni[i]=MIN(uni[i], UNI_MAX);
        uni[i]=MAX(uni[i], UNI_MIN);
        uni[i]/=UNI_MAX;
    }
}

void amp_process(void){
#if FILTER==1
    moving_average();
#elif FILTER==3
    kalman();
#endif

    data_unify();
}

void data_acquire(void){
    if(motor_t==0)
        speed_detection();
}
